package org.robotfish.geometry;

import org.ejml.simple.SimpleMatrix;
import org.robotfish.utils.SVGDocument;
import org.w3c.dom.Comment;
import org.w3c.dom.Element;

/**
 * Transforms points according to x and y coordinates
 * @author Mart
 *
 */
public class TransformationXY extends TransformationFork<TransformationX> {

	public TransformationXY(double start) {
		this.coordinateValue = start;
	}
	
	@Override
	public int getCoordinate() {
		return 2;
	}

	public void clearArcs() {
		for (TransformationX t:segments) {
			t.clearArcs();
		}
	}
	
	public void addLevels(double y[]) {
		for (double q:y) {
			add(new TransformationX(q));
		}
	}

	@Override
	public void appendSVGElements(SVGDocument doc, Element parent, double arcLength) {
		
		Comment comment = doc.createComment(getClass().getName()+".appendSVGElements: Y level curves="+segments.size());
		parent.appendChild(comment);

		Element text = doc.createText(getRay(), "z="+SVGDocument.DoubleToString3p(coordinateValue), doc.getColour(9), 10);
        parent.appendChild(text);
		
        // TODO kordinaattelgede joonistamine
        
		for (TransformationX t:segments) {
    		// create a group
            Element group = doc.createGroup();
            parent.appendChild(group);		
			t.appendSVGElements(doc, group, arcLength);
		}
        
	}

	public void addBackbone(SimpleMatrix noseRay, double alpha[], JointData jointData[]) {
		if (alpha.length != jointData.length)
			throw new RuntimeException("Arrays alpha and jointData has to have the same size");
		// TODO selgroo lisamine
		TransformationLine previous = new TransformationLine(0, noseRay);
		TransformationLine next;
		for (int i =0;i<alpha.length;i++) {
			SimpleMatrix joint = previous.getRay(jointData[i].xStart);
			joint.set(2,joint.get(2)+alpha[i]);
//			TransformationLine joint = new TransformationLine(jointData[0].xStart, joint);
			//next = 
			
			
		}
		
	}
	
/*	public void setY(double[] y) {
		
		for (double q:y) {
			
			
		}
		
		
	}*/
	
	
	/**
	 * Method for instance that has coordinate = 2 (Z)
	 * @param alpha0 enne põlve selgroo suund
	 * @param p liigendi asukohale vastav parameeter
	 * @param ray põlve väljuva osa suund
	 * @param stretchRegion Piirkonna raadius ümber murdekoha, mida venitatakse
	 */
	public void addBend(double alpha0, double p, SimpleMatrix ray, double stretchRegion) {
		
		// TODO modifitseerida töötama juhul, kui algne selgroog võib olla venitatud
		double alpha1 = ray.get(2);
		double beta = (alpha1 - alpha0)*0.5;
		double ds;
		SimpleMatrix oldDirectionRay = ray.copy();
		oldDirectionRay.set(2,alpha0);
/*		SimpleMatrix midDirectionRay = ray.copy();
		midDirectionRay.set(2,(alpha1 + alpha0)*0.5);*/
		double pStart = p-stretchRegion;
		double pEnd = p+stretchRegion;
		TransformationArc oldDirection = new TransformationLine(p, oldDirectionRay);
		TransformationArc oldDirectionStart = new TransformationLine(pStart, oldDirection.getRay(pStart));
//		TransformationArc midDirection = new TransformationLine(p, midDirectionRay);
		TransformationArc newDirection = new TransformationLine(p, ray);

		for (TransformationX t:segments) {
			double y = t.coordinateValue;
			if (beta*y>=0) { // terav nurk
				ds = 1-(Math.abs(y)/stretchRegion)*Math.tan(Math.abs(beta));
				
				t.add(new TrnStretchedLineB(pStart, oldDirection.getRay(pStart, y), ds));
				SimpleMatrix translatedRay = oldDirectionStart.getRay(pStart+stretchRegion*ds,y); 
				translatedRay.set(2, alpha1);
				t.add(new TrnStretchedLineE(p, translatedRay, ds));
				
			} else { // ümar nurk
				ds = 1+(Math.abs(y)/stretchRegion)*Math.abs(beta);
				double p1 = pStart+(stretchRegion                     )/ds;
				double p2 = pStart+(stretchRegion+(beta*2*Math.abs(y)))/ds;

				t.add(new TrnStretchedLineB(pStart, oldDirection.getRay(pStart, y), ds));
				t.add(new TransformationArc(p1, oldDirection.getRay(p, y), ds, -1/y));
				t.add(new TrnStretchedLineE(p2, newDirection.getRay(p, y), ds));
			}
			t.add(new TransformationLine(pEnd, newDirection.getRay(pEnd, y)));
		}
	}
	
}

class TransformationLine extends TransformationArc {
	/**
	 * Creates line with natural parameter as parameter.
	 * The line may be 0 length. 
	 * @param start the value of the curve parameter at the start of the arc
	 * @param ray start point and direction of the arc {x,y,direction}
	 */
	public TransformationLine(double start, SimpleMatrix ray) {
		super(start, ray, 1, 0);
	}
	@Override
	public int getColorIndex() {
		return 1;
	}
}

class TrnStretchedLineB extends TransformationArc {
	public TrnStretchedLineB(double start, SimpleMatrix ray, double ds) {
		super(start, ray, ds, 0);
	}
	@Override
	public int getColorIndex() {
		return 2;
	}
}

class TrnStretchedLineE extends TransformationArc {
	public TrnStretchedLineE(double start, SimpleMatrix ray, double ds) {
		super(start, ray, ds, 0);
	}
	@Override
	public int getColorIndex() {
		return 3;
	}
}
